
import sensor, image, lcd, time
from machine import UART
from fpioa_manager import fm
# 初始化UART1
from fpioa_manager import fm
from machine import UART
import sensor
import image
import time
import lcd

def init_uart():
    # 引脚映射
    fm.register(22, fm.fpioa.UART1_TX, force=True)  # GPIO22作为TX
    fm.register(23, fm.fpioa.UART1_RX, force=True)  # GPIO23作为RX
    return UART(UART.UART1, 115200, 8, 0, 0, timeout=1000, read_buf_len=256)

# 颜色阈值 (需要根据实际环境调整)
yellow_threshold = (36, 64, -49, -17, 6, 42)  # 绿色LAB阈值
red_threshold = (30, 100, 40, 127, -20, 50)     # 红色LAB阈值
green_threshold = (28, 62, 7, 53, -90, -49)    # 蓝色LAB阈值

# 初始化摄像头
clock = time.clock()
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(0)
sensor.run(1)
sensor.skip_frames(30)

# 初始化串口
uart = init_uart()
last_send_time = 0    # 上次发送时间
last_color = 0        # 上次发送的颜色编号
# ...（保留之前的初始化代码，颜色阈值设置和串口初始化部分）

while True:
    clock.tick()
    img = sensor.snapshot()
    fps = clock.fps()

    # 颜色识别逻辑
    current_color = 0
    max_size = 0
    target_blob = None  # 用于存储当前要标记的blob

    # 识别优先级：黄 -> 红 -> 绿
    for color_info in [
        (yellow_threshold, 1, (0,255,0)),  # 颜色阈值，编号，框颜色
        (red_threshold, 2, (255,0,0)),
        (green_threshold, 3, (0,0,255
        ))
    ]:
        blobs = img.find_blobs([color_info[0]],
                             pixels_threshold=100,
                             area_threshold=100,
                             merge=True)  # 开启区域合并

        if blobs:
            # 找最大色块
            max_b = max(blobs, key=lambda b: b.area())
            if max_b.area() > max_size:
                max_size = max_b.area()
                current_color = color_info[1]
                target_blob = max_b
                # 保存颜色信息用于绘制
                box_color = color_info[2]

    # 绘制识别结果（带文字说明）
    if target_blob:
        # 绘制双层框增强显示
        img.draw_rectangle(target_blob.rect(),
                         color=box_color,
                         thickness=3)
        img.draw_rectangle(target_blob.rect(),
                         color=(0,0,0),
                         thickness=1)

        # 在框上方显示颜色编号
        text = "Color:{current_color}"
        img.draw_string(target_blob.x()+2,
                      target_blob.y()-20,
                      text,
                      color=box_color,
                      scale=2,
                      mono_space=False)

    # 串口发送逻辑（保留之前的限流逻辑）
    if current_color != 0:
        current_time = time.ticks_ms()
        if current_color != last_color or (current_time - last_send_time) > 1000:
            uart.write(str(current_color).encode())
            last_send_time = current_time
            last_color = current_color

    # 显示帧率（移动到左下角）
    img.draw_string(2, img.height()-20,
                  "FPS:{fps:.1f}",
                  color=(255,0,0),
                  scale=1.5)
    lcd.display(img)
